package com.yuneec.droneservice.command;

import com.yuneec.droneservice.model.ControllerLocation;
import com.yuneec.droneservice.request.IResponse;
import com.yuneec.droneservice.utils.ByteUtils;

import java.util.Arrays;

/**
 * Created by gjw on 2017/3/24.
 */

public class GetControllerGPS extends BaseCommand {

    public GetControllerGPS() {
        super(MAV_RC_COMMAND.MAV_RC_CMD_GET_GPS);
    }

    @Override
    public IResponse<ControllerLocation> toResponse(byte[] data, int off, int len) {
        int code = data[off + 1] & 0xff;
        IResponse<ControllerLocation> response = new IResponse(ids(), code);
        if (code == MAV_RC_ERROR_CODE.MAV_RC_ERR_SUCCESS) {
            ControllerLocation controllerLocation = new ControllerLocation();
            controllerLocation.longitude = ByteUtils.byteArrayToInt(Arrays.copyOfRange(data, off + 2, off + 6), IS_BIGENDIAN) / 1e7f;
            controllerLocation.latitude = ByteUtils.byteArrayToInt(Arrays.copyOfRange(data, off + 6, off + 10), IS_BIGENDIAN) / 1e7f;
            controllerLocation.altitude = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 10, off + 12), IS_BIGENDIAN) / 100f;
            controllerLocation.satelliteCount = data[off + 12];
            controllerLocation.accuracy = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 13, off + 15), IS_BIGENDIAN);
            controllerLocation.speed = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 15, off + 17), IS_BIGENDIAN);
            controllerLocation.angle = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 17, off + 19), IS_BIGENDIAN);
            response.setParam(controllerLocation);
        }
        return response;
    }

}
